/*
 * PSDetectFormation.java
 *
 * Created on 18 de agosto de 2007, 08:35 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Behaviors.Schemas.Perception;
import Behaviors.Formations.FormationGenerator;
import Behaviors.Schemas.Schema;
import Behaviors.Schemas.Vector2dSchema;
import Robot.Sensors.Vector2DSensor;
import Util.Vector2d;
import javaclient2.Position2DInterface;
import Robot.Position2d;
import Util.Pose2d;
/**
 *
 * @author jonathan
 */
public class PSDetectFormation extends Schema implements Vector2dSchema {
    Position2d self;
    Position2d center;
    int robotId;
    FormationGenerator form;
    
    private float centroidDist = 3.0f;
    /**
     * Creates a new instance of PSDetectFormation
     */
    public PSDetectFormation(String name, FormationGenerator formation,Position2d self,Position2d center) {
        super(name);
        this.self=self;
        this.center=center;
        robotId=self.getRobotId();
        form=formation;
    }
    
    public Vector2d Value(){
        Pose2d myPosition = self.read(Position2d.READ_MODE_NO_WAIT);
        Pose2d centerPosition = center.read(Position2d.READ_MODE_NO_WAIT);
        
        Vector2d centerPos=new Vector2d(centerPosition.getPx(),centerPosition.getPy(),Vector2d.MODE_XY);
        Vector2d myPos=new Vector2d(myPosition.getPx(),myPosition.getPy(),Vector2d.MODE_XY);
        Vector2d formationPos=form.getPositionForRobot(robotId-1);
        
        
        float a = centerPosition.getPa();
         if(formationPos.equals(null))
             System.out.println("formation pos null");
         
        try{
            formationPos=formationPos.rotate(a);
        }
        catch(NullPointerException e){
            System.out.println("formation pos null");
        }
        
        
        Vector2d ret;
        
        Vector2d temp = new Vector2d(centroidDist,centerPosition.getPa(),Vector2d.MODE_MA);
        temp = centerPos.sub(temp); 
        //el formation pos esta mostrando error!!
        ret = temp.add(formationPos);
        //System.out.println(formationPos.toString()+" Robot: "+robotId);
        return ret;

    }
}
